/*****************************************************************************
** Includes
*****************************************************************************/

#include "RobotCmdNode.hpp"
#include <QString>
#include "RobotServerPC.h"

/*****************************************************************************
** Namespaces
*****************************************************************************/
using namespace std::chrono_literals;
using std::placeholders::_1;

/*****************************************************************************
** Implementation
*****************************************************************************/

RobotCmdNode::RobotCmdNode(RobotServerPC* rspc)
    : Node("robotserverpc_node"), m_rspc(rspc)
{
    m_pubCmd = this->create_publisher<pccmd_msg::msg::PcCmd>("PcCmdMsg", 10);

    m_pubClawCmd =
        this->create_publisher<clawcmd_msg::msg::ClawCmd>("ClawCmd", 10);

    m_pubRsSwitch =
        this->create_publisher<rsswitch_msg::msg::RsSwitch>("RsSwitch", 10);

    m_pubRsPos = this->create_publisher<rspos_msg::msg::RsPos>("RsPos", 10);

    // m_subParam = this->create_subscription<param_msg::msg::Param>(
    //     "ParamMsg", 1, std::bind(&RobotCmdNode::parammsg_callback, this, _1));

    m_subSlamReady = this->create_subscription<slamready_msg::msg::SlamReady>(
        "SlamReady", 10, std::bind(&RobotCmdNode::slamready_callback, this, _1));

    m_subSlamPos = this->create_subscription<slampos_msg::msg::SlamPos>(
        "SlamPos", 10, std::bind(&RobotCmdNode::slampos_callback, this, _1));
}

RobotCmdNode::~RobotCmdNode()
{
    rclcpp::shutdown();
}

void RobotCmdNode::sendCmd(const std::string& cmdname,
                           const std::string& cmdcontent)
{
    auto cmd_msg = pccmd_msg::msg::PcCmd();
    cmd_msg.cmdname = cmdname;
    cmd_msg.cmdcontent = cmdcontent;
    m_pubCmd->publish(cmd_msg);

    RCLCPP_INFO(this->get_logger(),
                "RobotCmdNode SendCmd, cmdname: %s, cmdcontent: %s",
                cmdname.c_str(), cmdcontent.c_str());
}

void RobotCmdNode::rsswitch(bool b)
{
    auto cmd = rsswitch_msg::msg::RsSwitch();
    cmd.state = b ? 1 : 0;
    m_pubRsSwitch->publish(cmd);
    RCLCPP_INFO(this->get_logger(), "pub rsswitch b: %d", cmd.state);
}

void RobotCmdNode::setSendRsPos(float xl,
                                float yl,
                                float zl,
                                float ll,
                                float wl,
                                float hl,
                                float xr,
                                float yr,
                                float zr,
                                float lr,
                                float wr,
                                float hr,
                                std::string serial)
{
    auto cmd = rspos_msg::msg::RsPos();
    cmd.xl = xl;
    cmd.yl = yl;
    cmd.zl = zl;
    cmd.ll = ll;
    cmd.wl = wl;
    cmd.hl = hl;
    cmd.xr = xr;
    cmd.yr = yr;
    cmd.zr = zr;
    cmd.lr = lr;
    cmd.wr = wr;
    cmd.hr = hr;
    cmd.serial = serial;

    m_pubRsPos->publish(cmd);
    RCLCPP_INFO(this->get_logger(), "rspos pulish");
}

// void RobotCmdNode::parammsg_callback(
//     const param_msg::msg::Param::SharedPtr msg) const
// {
//     std::string strCmdName = msg->cmdname;
//     std::vector<std::string> paramVec = msg->params;

//     //if (strCmdName == "trigger") {
//     //    int nLeftTrigger = atoi(paramVec.at(0).c_str());
//     //    int nRightTigger = atoi(paramVec.at(1).c_str());
//     //    m_rspc->call_tirgger(nLeftTrigger, nRightTigger);
//     //} else if (strCmdName == "setGenRParam") {
//     //    int nOperate = atoi(paramVec.at(0).c_str());
//     //    std::string strSerial = paramVec.at(1);

//     //    m_rspc->call_setGenRParam(nOperate, QString::fromStdString(strSerial));
//     //} else if (strCmdName == "opreate") {
//     //    int nExecStep = atoi(paramVec.at(0).c_str());
//     //    std::string strSerial = paramVec.at(1);

//     //    m_rspc->call_opreate(nExecStep, QString::fromStdString(strSerial));
//     //}
// }

void RobotCmdNode::slamready_callback(
    const slamready_msg::msg::SlamReady::SharedPtr msg) const
{
    m_rspc->call_processSLAM();
}

void RobotCmdNode::slampos_callback(
    const slampos_msg::msg::SlamPos::SharedPtr msg) const
{
    float x = msg->x;
    float y = msg->y;
    float z = msg->z;
    float rx = msg->rx;
    float ry = msg->ry;
    float rz = msg->rz;

    m_rspc->call_slamPos(x, y, z, rx, ry, rz);
}

void RobotCmdNode::clawControl(uint8_t claw_dir, uint8_t claw_force)
{
    auto clawcmd_msg = clawcmd_msg::msg::ClawCmd();
    clawcmd_msg.claw_dir = claw_dir;
    clawcmd_msg.claw_force = claw_force;
    m_pubClawCmd->publish(clawcmd_msg);

    RCLCPP_INFO(this->get_logger(),
                "RobotCmdNode claw control, claw dir: %d, claw force: %d",
                claw_dir, claw_force);
}
